
#include <Arduino.h>
#include "../inc/firmataC.h"

#include <string.h>
#include <stdlib.h>
#include <stdio.h>

void firmata_client::begin(Stream *serial)
{
  this->serial = serial;
  // serial_open(res->serial, name);
  firmata_initPins();
  // serial_setBaud(res->serial, 57600);
  firmata_askFirmware();
}

int firmata_client::firmata_pull()
{
  uint8_t buff[FIRMATA_MSG_LEN];
  int r;

  r = serial->available();
  if (r > 0)
  {
    r = serial->readBytes(buff, sizeof(buff));
    if (r < 0)
    {
      return (0);
    }
    if (r > 0)
    {
      firmata_parse( buff, r);
      return (r);
    }
  }
  else if (r < 0)
  {
    return (r);
  }
  return 0;
}

void firmata_client::firmata_parse( const uint8_t *buf, int len)
{
  const uint8_t *p;
  const uint8_t *end;

  p = buf;
  end = p + len;
  for (p = buf; p < end; p++)
  {
    uint8_t msn = *p & 0xF0;
    if (msn == 0xE0 || msn == 0x90 || *p == 0xF9)
    {
      parse_command_len = 3;
      parse_count = 0;
    }
    else if (msn == 0xC0 || msn == 0xD0)
    {
      parse_command_len = 2;
      parse_count = 0;
    }
    else if (*p == FIRMATA_START_SYSEX)
    {
      parse_count = 0;
      parse_command_len = sizeof(parse_buff);
    }
    else if (*p == FIRMATA_END_SYSEX)
    {
      parse_command_len = parse_count + 1;
    }
    else if (*p & 0x80)
    {
      parse_command_len = 1;
      parse_count = 0;
    }
    if (parse_count < (int)sizeof(parse_buff))
    {
      parse_buff[parse_count] = (uint8_t)(*p);
      parse_count++;
    }
    if (parse_count == parse_command_len)
    {
      firmata_endParse();
      parse_count = 0;
      parse_command_len = 0;
    }
  }
}

void firmata_client::firmata_endParse()
{
  uint8_t cmd = (parse_buff[0] & 0xF0);
  int pin;

  if (cmd == 0xE0 && parse_count == 3)
  {
    int analog_ch = (parse_buff[0] & 0x0F);
    int analog_val = parse_buff[1] | (parse_buff[2] << 7);
    for (pin = 0; pin < 128; pin++)
    {
      if (pins[pin].analog_channel == analog_ch)
      {
        pins[pin].value = analog_val;
        return;
      }
    }
    return;
  }
  if (cmd == 0x90 && parse_count == 3)
  {
    int port_num = (parse_buff[0] & 0x0F);
    int port_val = parse_buff[1] | (parse_buff[2] << 7);
    int pin = port_num * 8;
    int mask;
    for (mask = 1; mask & 0xFF; mask <<= 1, pin++)
    {
      if (pins[pin].mode == MODE_INPUT)
      {
        uint32_t val = (port_val & mask) ? 1 : 0;
        pins[pin].value = val;
      }
    }
    return;
  }
  if (parse_buff[0] == FIRMATA_START_SYSEX && parse_buff[parse_count - 1] == FIRMATA_END_SYSEX)
  {
    if (parse_buff[1] == FIRMATA_REPORT_FIRMWARE)
    {
      int len = 0;
      int i;
      for (i = 4; i < parse_count - 2; i += 2)
      {
        firmware[len++] = (parse_buff[i] & 0x7F) | ((parse_buff[i + 1] & 0x7F) << 7);
      }
      firmware[len++] = '-';
      firmware[len++] = parse_buff[2] + '0';
      firmware[len++] = '.';
      firmware[len++] = parse_buff[3] + '0';
      firmware[len++] = 0;
      printf("Name :: %s\n", firmware);
      // query the board's capabilities only after hearing the
      // REPORT_FIRMWARE message.  For boards that reset when
      // the port open (eg, Arduino with reset=DTR), they are
      // not ready to communicate for some time, so the only
      // way to reliably query their capabilities is to wait
      // until the REPORT_FIRMWARE message is heard.
      uint8_t buf[80];
      len = 0;
      buf[len++] = FIRMATA_START_SYSEX;
      buf[len++] = FIRMATA_ANALOG_MAPPING_QUERY; // read analog to pin # info
      buf[len++] = FIRMATA_END_SYSEX;
      buf[len++] = FIRMATA_START_SYSEX;
      buf[len++] = FIRMATA_CAPABILITY_QUERY; // read capabilities
      buf[len++] = FIRMATA_END_SYSEX;
      for (i = 0; i < 16; i++)
      {
        buf[len++] = 0xC0 | i; // report analog
        buf[len++] = 1;
        buf[len++] = 0xD0 | i; // report digital
        buf[len++] = 1;
      }
      isReady = 1;
      serial->write(buf, len);
    }
    else if (parse_buff[1] == FIRMATA_CAPABILITY_RESPONSE)
    {
      int pin, i, n;
      for (pin = 0; pin < 128; pin++)
      {
        pins[pin].supported_modes = 0;
      }
      for (i = 2, n = 0, pin = 0; i < parse_count; i++)
      {
        if (parse_buff[i] == 127)
        {
          pin++;
          n = 0;
          continue;
        }
        if (n == 0)
        {
          // first byte is supported mode
          pins[pin].supported_modes |= (1 << parse_buff[i]);
        }
        n = n ^ 1;
      }
      // send a state query for for every pin with any modes
      for (pin = 0; pin < 128; pin++)
      {
        uint8_t buf[512];
        int len = 0;
        if (pins[pin].supported_modes)
        {
          buf[len++] = FIRMATA_START_SYSEX;
          buf[len++] = FIRMATA_PIN_STATE_QUERY;
          buf[len++] = pin;
          buf[len++] = FIRMATA_END_SYSEX;
        }
        serial->write(buf, len);
      }
    }
    else if (parse_buff[1] == FIRMATA_ANALOG_MAPPING_RESPONSE)
    {
      int pin = 0;
      int i;
      for (i = 2; i < parse_count - 1; i++)
      {
        pins[pin].analog_channel = parse_buff[i];
        pin++;
      }
      return;
    }
    else if (parse_buff[1] == FIRMATA_PIN_STATE_RESPONSE && parse_count >= 6)
    {
      int pin = parse_buff[2];
      pins[pin].mode = parse_buff[3];
      pins[pin].value = parse_buff[4];
      if (parse_count > 6)
        pins[pin].value |= (parse_buff[5] << 7);
      if (parse_count > 7)
        pins[pin].value |= (parse_buff[6] << 14);
    }
    return;
  }
}

void firmata_client::firmata_initPins()
{
  int i;

  parse_count = 0;
  parse_command_len = 0;
  isReady = 0;
  for (i = 0; i < 128; i++)
  {
    pins[i].mode = 255;
    pins[i].analog_channel = 127;
    pins[i].supported_modes = 0;
    pins[i].value = 0;
  }
}

int firmata_client::firmata_askFirmware()
{
  uint8_t buf[3];
  int res;

  buf[0] = FIRMATA_START_SYSEX;
  buf[1] = FIRMATA_REPORT_FIRMWARE; // read firmata name & version
  buf[2] = FIRMATA_END_SYSEX;
  res = serial->write(buf, 3);
  return (res);
}

int firmata_client::firmata_pinMode( int pin, int mode)
{
  int res;
  uint8_t buff[4];

  pins[pin].mode = mode;
  buff[0] = FIRMATA_SET_PIN_MODE;
  buff[1] = pin;
  buff[2] = mode;
  printf("Setting pinMode at: %i with value: %i\n", pin, mode);
  res = serial->write(buff, 3);
  return (res);
}

int firmata_client::firmata_analogWrite( int pin, int value)
{
  int res;

  uint8_t buff[3];
  printf("Writting analogWrite at: %i with value: %i\n", pin, value);
  buff[0] = 0xE0 | pin;
  buff[1] = value & 0x7F;
  buff[2] = (value >> 7) & 0x7F;
  res = serial->write(buff, 3);
  return (res);
}

int firmata_client::firmata_digitalWrite( int pin, int value)
{
  int i;
  int res;
  uint8_t buff[4];

  if (pin < 0 || pin > 127)
    return (0);
  pins[pin].value = value;
  int port_num = pin / 8;
  int port_val = 0;
  for (i = 0; i < 8; i++)
  {
    int p = port_num * 8 + i;
    if (pins[p].mode == MODE_OUTPUT || pins[p].mode == MODE_INPUT)
    {
      if (pins[p].value)
      {
        port_val |= (1 << i);
      }
    }
  }
  printf("Writting digitalWrite at: %i with value: %i\n", pin, value);
  buff[0] = FIRMATA_DIGITAL_MESSAGE | port_num;
  buff[1] = port_val & 0x7F;
  buff[2] = (port_val >> 7) & 0x7F;
  res = serial->write(buff, 3);
  return (res);
}
